import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    ExecuteProcess,
    SetEnvironmentVariable
)
from launch.substitutions import LaunchConfiguration


def generate_launch_description():

    pkg_simulator = get_package_share_directory("robot_gazebo_sim")

    world_path = LaunchConfiguration("world_path")

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        "use_sim_time",
        default_value="True",
        description="Use simulation (Gazebo) clock if true",
    )

    declare_world_sdf_path = DeclareLaunchArgument(
        "world_sdf_path",
        default_value=os.path.join(pkg_simulator, "resource", "worlds", "empty.world"),
        description="Path to the world SDF file",
    )

    declare_simulator_cmd = DeclareLaunchArgument(
        "headless", default_value="False", description="Whether to execute gzclient)"
    )

    set_enviroment_plugin = SetEnvironmentVariable(
        name="GAZEBO_PLUGIN_PATH",
        value=os.path.join(
            get_package_share_directory("gazebo_ros2_tracked_vehicle_interface")
        ),
    )

    # 启动Gazebo的命令
    start_gazebo_cmd = ExecuteProcess(
        cmd=[
            "gazebo",
            "--verbose",
            "-s",
            "libgazebo_ros_init.so",
            "-s",
            "libgazebo_ros_factory.so",
            world_path,
        ],
        cwd=[pkg_simulator],
        output="screen",
    )


    # 创建LaunchDescription对象并添加所有声明和执行命令
    ld = LaunchDescription()
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_world_sdf_path)
    ld.add_action(declare_simulator_cmd)
    ld.add_action(set_enviroment_plugin)
    ld.add_action(start_gazebo_cmd)

    # 返回LaunchDescription对象
    return ld
